#ifndef FRICTION_MODEL_IMPL_H
#define FRICTION_MODEL_IMPL_H

#include <iostream>
#include <vector>
#include <cmath>
#include "FrictionModel.h"
#include "ompl/util/RandomNumbers.h"

class FrictionModelImpl : public FrictionModel {

  mutable ompl::RNG rng;
  PlayerCc::SimulationProxy &sim;
  double errorRate;

  bool is_within_spill_range(const double x, const double y,
    const double sx, const double sy) const {
    return ((x-sx)*(x-sx) + (y-sy)*(y-sy)) < (0.5 * 0.5);
  }

  double detectWaterWithFalseNegative() const {
    if (rng.uniform01() > errorRate) {
      return 0.5; // correct
    } else {
      return 1.0; // false
    }
  }

  double detectNothingWithFalsePositive() const {
    if (rng.uniform01() > errorRate) {
      return 1.0; // correct
    } else {
      return 0.5; // false
    }
  }

  typedef std::vector< std::pair<double,double> > spillpos_t;
  spillpos_t spillpos;

public:

  FrictionModelImpl(PlayerCc::SimulationProxy & sim)
  : sim(sim), errorRate(0.2)
  {
    // cache spill locations. i.e. only works one time
    static const char * spills [] = {
      "water1", "water2", "water3", "water4", 0
    };
    for (int i = 0; spills[i]; ++i) {
      double sx, sy, a;
      sim.GetPose2d(const_cast<char *>(spills[i]), sx, sy, a);
      std::cout << spills[i] << " at (" << sx << ", " << sy << ")" << std::endl;
      spillpos.push_back(std::make_pair(sx,sy));
    }
    std::cout << "sensor error rate " << errorRate << std::endl;
  }

  virtual double probability(const double x, const double y) const {
    double prob = 0;
    int n = 0;

    // for each spill
    for (spillpos_t::const_iterator i = spillpos.begin(); i != spillpos.end(); ++i) {
      if (is_within_spill_range(x, y, i->first, i->second)) {
        // positive
        prob += detectWaterWithFalseNegative();
        ++n;
      }
    }

    // one or more spills encountered, normalize
    if (n > 0) {
      prob /= (double) n;
    }

    // no spills encountered
    if (n == 0) {
      prob = detectNothingWithFalsePositive();
    }

    return prob;
  }
};

#endif
